import cv2
import numpy as np
import yaml

# Load the images
img1 = cv2.imread('/home/daybeha/Documents/rosbag_tools/calibration/results/left/53.jpg')
img2 = cv2.imread('/home/daybeha/Documents/rosbag_tools/calibration/results/right/53.jpg')
h, w = img1.shape[:2]

# 创建cv2.FileStorage对象
fs_settings_l = cv2.FileStorage('./build/left.yaml', cv2.FILE_STORAGE_READ)
fs_settings_r = cv2.FileStorage('./build/right.yaml', cv2.FILE_STORAGE_READ)

# 检查是否成功打开文件
if not fs_settings_l.isOpened() or not fs_settings_r.isOpened():
    print("ERROR: Wrong path to settings")
    exit(-1)

# 读取文件中的数据
# camera matrix
camera_matrix_l = fs_settings_l.getNode("camera_matrix").mat()
camera_matrix_r = fs_settings_r.getNode("camera_matrix").mat()

# distortion coefficients
dist_coeffs_l = fs_settings_l.getNode("distortion_coefficients").mat()
dist_coeffs_r = fs_settings_r.getNode("distortion_coefficients").mat()

# rotation matrix
R_l = fs_settings_l.getNode("rectification_matrix").mat()
R_r = fs_settings_r.getNode("rectification_matrix").mat()

# projection matrix
P_l = fs_settings_l.getNode("projection_matrix").mat()
P_r = fs_settings_r.getNode("projection_matrix").mat()

mapx_l, mapy_l = cv2.initUndistortRectifyMap(camera_matrix_l, dist_coeffs_l, R_l, P_l, (w, h), cv2.CV_32FC1)
mapx_r, mapy_r = cv2.initUndistortRectifyMap(camera_matrix_r, dist_coeffs_r, R_r, P_r, (w, h), cv2.CV_32FC1)

img1 = cv2.remap(img1, mapx_l, mapy_l, cv2.INTER_LINEAR)
img2 = cv2.remap(img2, mapx_r, mapy_r, cv2.INTER_LINEAR)
cv2.imshow("left", img1)
cv2.imshow("right", img2)
cv2.waitKey()


